/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.ortn.wildbots.commands;

import edu.ortn.wildbots.commands.CameraControl;
import edu.ortn.wildbots.commands.CommandBase;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Joystick;

/**
 *
 * @author Stephen
 */
public class Operate extends CommandBase {
    
    private Joystick joyLeft;
    private Joystick joyRight;
    
    private long millis;
    
    private boolean tankMode = true;
    
    private CameraControl cc;
    
    public Operate(CameraControl cc) {
        requires(chassis);
        this.cc = cc;
        
        joyRight = oi.getJoystickRight();
        joyLeft = oi.getJoystickLeft();
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        chassis.tankDrive(0, 0);
        camMount.setCenter();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        long time = System.currentTimeMillis();
        if (time - millis > 200) {
            /*DriverStationLCD lcd = DriverStationLCD.getInstance();
            String[] chassisData = chassis.getData();
            lcd.println(DriverStationLCD.Line.kMain6, 1, chassisData[0]);
            lcd.println(DriverStationLCD.Line.kUser2, 1, chassisData[1]);
            lcd.println(DriverStationLCD.Line.kUser3, 1, chassisData[2]);
             * */
             
            
            //String[] cameraData = camMount.getData();
            //lcd.println(DriverStationLCD.Line.kUser4, 1, cameraData[0]);
            //lcd.println(DriverStationLCD.Line.kUser5, 1, cameraData[1]);
            //lcd.println(DriverStationLCD.Line.kUser6, 1, cameraData[2]);
            //lcd.updateLCD();
            
            millis = time;
        }
        buttonCheck();
        cameraCheck();
        move();
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
        camMount.setCenter();
        chassis.arcadeDrive(0, 0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }

    private void buttonCheck() {
        if (tankMode == true && joyRight.getRawButton(6)) {
            tankMode = false;
        }
        else if (joyRight.getRawButton(6)) {
            tankMode = true;
        }
    }

    private void move() {
        
        //chassis.setSpeed(-(joyLeft.getRawAxis(3)+1)/2);
        
        if (tankMode) {
            if (chassis.getDistanceInInches() < 20 &&
                    joyLeft.getY() > 0 && joyRight.getY() > 0) {
                chassis.tankDrive(0, 0);
            }
            else {
                chassis.tankDrive(joyLeft.getY(), joyRight.getY());
            }
        } 
        else {
            if (chassis.getDistanceInInches() < 20 && 
                    joyRight.getX() < 0.15 && joyRight.getY() > 0) {
                chassis.arcadeDrive(0, 0);
            }
            else {
                chassis.arcadeDrive(joyRight.getY(), joyRight.getX());
            }            
        }
        
    }

    private void cameraCheck() {    //check buttons for camera operations
        
        //camMount.setSpeed((1-(joyRight.getRawAxis(3)+1)/2)*10);

        //System.out.println((1-(joyRight.getRawAxis(3)+1)/2)*10);
        if(joyRight.getRawButton(4)){
            cc.moveLeft();
        }
        if(joyRight.getRawButton(5)){
            cc.moveRight();
        }
        if(joyRight.getRawButton(2)){
            cc.moveDown();
        }
        if(joyRight.getRawButton(3)){
            cc.moveUp();
        }
        if (joyRight.getRawButton(1)) {// Trigger
           cc.setCenter(); 
        }
    }
}
